#!/bin/bash
# author: TFly
# 将此脚本放在 PX4 源码目录下

px4_dir=`pwd`
envfile="$HOME/.bashrc"

# env
simulation_dir="$px4_dir/Tools/sitl_gazebo"
sitl_build_dir="$px4_dir/build/px4_sitl_default"
gazebo_env="$px4_dir/Tools/setup_gazebo.bash"

# 目标版本
target_version="1.14.0"
version=$(git describe --tags --always)
version_nums=$(echo $version | sed 's/^v//')

echo "--- PX4 Firmware Version: $version"
echo "--- PX4 Dir: $px4_dir"


# 比较版本号
if [ "$(printf '%s\n' "$target_version" "$version_nums" | sort -V | head -n1)" = "$target_version" ]; then
	simulation_dir="$px4_dir/Tools/simulation/gazebo-classic"
	sitl_build_dir="$px4_dir/build/px4_sitl_default"
	gazebo_env="$px4_dir/Tools/simulation/gazebo-classic/setup_gazebo.bash"
fi

function check_dir(){
	if [ ! -d "$1" ]; then
    	echo "目录 '$1' 不存在。"
		exit 1
	fi
}

function check_file(){
	if [ ! -e "$1" ]; then
    	echo "文件 '$1' 不存在。"
		exit 1
	fi
}

function normalize_to_relative() {
    local path="$1"
    # 将主目录路径替换为 ~
    path=$(echo "$path" | sed "s|^$HOME|~|")
    # 如果需要保持 $HOME 形式，可以再次替换回去
    # 注意：这里我们选择保留 ~ 形式，但你可以根据需要调整
    echo "$path"
}

function addUserRosEnv(){
	if [ -e "$1" ]; then
        if ! grep -Fxq "$1" "$2"; then
    		temp=$(normalize_to_relative "$1")
    		if ! grep -Fxq "$temp" "$2"; then
    			echo "--- $temp 已经被包含在 $2 中，跳过"
    		else
        		echo "source $1" >> "$2"
        	fi
    	else
        	echo "--- $1 已经被包含在 $2 中，跳过"
    	fi
    else
        echo "文件 $1 不存在, 并不影响PX4环境, 跳过。"
    fi
}

function addPathIfNotExists(){
    if ! grep -Fxq "$1" "$2"; then
    	temp=$(normalize_to_relative "$1")
    	if ! grep -Fxq "$temp" "$2"; then
    		echo "--- $temp 已经被包含在 $2 中，跳过"
    	else
        	echo "$1" >> "$2"
        fi
    else
        echo "$1 已经被包含在 $2 中，跳过"
    fi
}

check_file $envfile
check_dir $simulation_dir
check_dir $sitl_build_dir
check_file $gazebo_env

addUserRosEnv "$HOME/catkin_ws/devel/setup.bash" $envfile
addPathIfNotExists "source $gazebo_env $px4_dir/ $sitl_build_dir" $envfile
addPathIfNotExists "export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:$px4_dir" $envfile
addPathIfNotExists "export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:$simulation_dir" $envfile
echo "--- 环境配置完成，请运行 source ~/.bashrc 检查环境是否配置成功."



